#include <DATASCOPE.h>
#include <PID_v1.h>

/* 波形显示库相关 */
DATASCOPE scope;

/* PID 库相关 */
float dl = 0;
double kp = 50;
double ki = 0;
double kd = 0.5;
double target;
double input = 0;
double output = 0;
PID AccurateRotation90(&input, &output, &target, kp, ki, kd, DIRECT);

/* 电机控制相关 */
int STBY = 6;       /* STBY为正常工作/待机状态控制引脚 */
int AIN1 = 7;       /* 电机驱动引脚 */
int AIN2 = 8;       /* 电机驱动引脚 */
int PWMA = 10;      /* 电机驱动PWM */
int ENCODEA = 3;    /* 电机测速编码器计数引脚 */
int ENCODEB = 11;   /* 电机测速编码器计数引脚 */
int count = 0;      /* 编码器旋转计数, 正转++, 逆转-- */

void setup()
{
  /* 设置引脚均为输出模式 */
  pinMode(STBY, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  /* 设置PID参数 */
  AccurateRotation90.SetTunings(kp, ki, kd);
  AccurateRotation90.SetOutputLimits(-255 + dl, 255 - dl);
  AccurateRotation90.SetSampleTime(5);
  AccurateRotation90.SetMode(AUTOMATIC);

  /* 设置 PIN3 为外部中断引脚, 中断触发方式为上升沿触发 */
  attachInterrupt(digitalPinToInterrupt(ENCODEA), PIN3ISR, RISING);

  /* 设置波形显示波特率 */
  scope.baudrate(128000);

  /* 设置 TB6112 为正常工作模式 */
  digitalWrite(STBY, HIGH);

  /* 设置电机正转 */
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  analogWrite(PWMA, 0);
}

void loop()
{
  int pwm = 0;

  input = count;

  /**
     电机光电编码器30齿, 减速箱减速比1:48
     因此, 转动90度对应编码器上升沿个数为 30*48/4 = 360
  */
  target = 360;
  AccurateRotation90.Compute();
  if (output > 0)
  {
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
  }
  else
  {
    output = 0 - output;
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
  }
  analogWrite(PWMA, output);

  scope.update(1, input);
  scope.put(1);

  delay(5);
}

void PIN3ISR(void)
{
  if (0 == digitalRead(ENCODEB))
  {
    count++;
  }
  else if (1 == digitalRead(ENCODEB))
  {
    count--;
  }
}
